#include "EMatrix4x4.h"
#include <cmath>
#include <limits>

#define SET_ROW(row, v1, v2, v3, v4 )    \
    data[(row)][0] = (v1); \
    data[(row)][1] = (v2); \
    data[(row)][2] = (v3); \
    data[(row)][3] = (v4);

EMatrix4x4::EMatrix4x4()
{
    this->makeIdentity();
}

EMatrix4x4::~EMatrix4x4()
{
}

EMatrix4x4& EMatrix4x4::operator=(const EMatrix4x4& value)
{
    for(int i = 0 ; i < 4; i++)
    {
        for(int j = 0 ; j< 4; j++)
        {
            data[i][j] = value.data[i][j];
        }
    }

    return *this;
}

void EMatrix4x4::makeIdentity()
{
    SET_ROW(0,    1, 0, 0, 0 )
    SET_ROW(1,    0, 1, 0, 0 )
    SET_ROW(2,    0, 0, 1, 0 )
    SET_ROW(3,    0, 0, 0, 1 )
}

void EMatrix4x4::makeTranslate(double x, double y, double z)
{
    SET_ROW(0,    1, 0, 0, 0 )
    SET_ROW(1,    0, 1, 0, 0 )
    SET_ROW(2,    0, 0, 1, 0 )
    SET_ROW(3,    x, y, z, 1 )
}

void EMatrix4x4::makeTranslate(const EVec3& trans)
{
    this->makeTranslate(trans.x, trans.y, trans.z);
}

void EMatrix4x4::setTranslate(double x, double y, double z)
{
    SET_ROW(3,    x, y, z, 1 )
}

void EMatrix4x4::setTranslate(const EVec3& trans)
{
    this->setTranslate(trans.x, trans.y, trans.z);
}

void EMatrix4x4::setRotate(const EVec4& quat)
{
    EVec3 trans = this->getTrans();
    this->makeRotate(quat);
    this->setTranslate(trans);
}

void EMatrix4x4::makeScale(double x, double y, double z)
{
    SET_ROW(0,    x, 0, 0, 0 )
    SET_ROW(1,    0, y, 0, 0 )
    SET_ROW(2,    0, 0, z, 0 )
    SET_ROW(3,    0, 0, 0, 1 )
}

void EMatrix4x4::makeScale(const EVec3& scale)
{
    this->makeTranslate(scale.x, scale.y, scale.z);
}

void EMatrix4x4::makeRotate( double angle, double x, double y, double z )
{
    makeIdentity();

    EVec4 quat;
    quat.makeRotate( angle, x, y, z);
    makeRotate(quat);
}

void EMatrix4x4::makeRotate( double angle1, const EVec3& axis1,
            double angle2, const EVec3& axis2,
            double angle3, const EVec3& axis3)
{
}

void EMatrix4x4::makeRotate(const EVec4& q)
{
    double length2 = q.length2();
    if (fabs(length2) <= std::numeric_limits<double>::min())
    {
        data[0][0] = 0.0; data[1][0] = 0.0; data[2][0] = 0.0;
        data[0][1] = 0.0; data[1][1] = 0.0; data[2][1] = 0.0;
        data[0][2] = 0.0; data[1][2] = 0.0; data[2][2] = 0.0;
    }
    else
    {
        double rlength2;
        // normalize quat if required.
        // We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
        // That is a square of a square root, so it is possible to avoid that
        if (length2 != 1.0)
        {
            rlength2 = 2.0/length2;
        }
        else
        {
            rlength2 = 2.0;
        }

        // Source: Gamasutra, Rotating Objects Using Quaternions
        //
        //http://www.gamasutra.com/features/19980703/quaternions_01.htm

        double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

        // calculate coefficients
        x2 = rlength2*q.x;
        y2 = rlength2*q.y;
        z2 = rlength2*q.z;

        xx = q.x * x2;
        xy = q.x * y2;
        xz = q.x * z2;

        yy = q.y * y2;
        yz = q.y * z2;
        zz = q.z * z2;

        wx = q.w * x2;
        wy = q.w * y2;
        wz = q.w * z2;

        // Note.  Gamasutra gets the matrix assignments inverted, resulting
        // in left-handed rotations, which is contrary to OpenGL and OSG's
        // methodology.  The matrix assignment has been altered in the next
        // few lines of code to do the right thing.
        // Don Burns - Oct 13, 2001
        data[0][0] = 1.0 - (yy + zz);
        data[1][0] = xy - wz;
        data[2][0] = xz + wy;


        data[0][1] = xy + wz;
        data[1][1] = 1.0 - (xx + zz);
        data[2][1] = yz - wx;

        data[0][2] = xz - wy;
        data[1][2] = yz + wx;
        data[2][2] = 1.0 - (xx + yy);
    }

}

EVec4 EMatrix4x4::getQuat()
{
    EVec4 q;

    double QW, QX, QY, QZ;
    double s;
    double tq[4];
    int i, j;

    // Use tq to store the largest trace
    tq[0] = 1 + data[0][0]+data[1][1]+data[2][2];
    tq[1] = 1 + data[0][0]-data[1][1]-data[2][2];
    tq[2] = 1 - data[0][0]+data[1][1]-data[2][2];
    tq[3] = 1 - data[0][0]-data[1][1]+data[2][2];

    // Find the maximum (could also use stacked if's later)
    j = 0;
    for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;

    // check the diagonal
    if (j==0)
    {
        /* perform instant calculation */
        QW = tq[0];
        QX = data[1][2]-data[2][1];
        QY = data[2][0]-data[0][2];
        QZ = data[0][1]-data[1][0];
    }
    else if (j==1)
    {
        QW = data[1][2]-data[2][1];
        QX = tq[1];
        QY = data[0][1]+data[1][0];
        QZ = data[2][0]+data[0][2];
    }
    else if (j==2)
    {
        QW = data[2][0]-data[0][2];
        QX = data[0][1]+data[1][0];
        QY = tq[2];
        QZ = data[1][2]+data[2][1];
    }
    else /* if (j==3) */
    {
        QW = data[0][1]-data[1][0];
        QX = data[2][0]+data[0][2];
        QY = data[1][2]+data[2][1];
        QZ = tq[3];
    }

    s = std::sqrt(0.25/tq[j]);
    QW *= s;
    QX *= s;
    QY *= s;
    QZ *= s;

    q.w = QW;
    q.x = QX;
    q.y = QY;
    q.z = QZ;

    return q;
}

EVec3 EMatrix4x4::getEulerAngle()
{
    double sy = sqrt(
            this->data[0][0] * this->data[0][0]+this->data[0][1] * this->data[0][1]);
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular)
    {
        x = atan2( this->data[1][2], this->data[2][2]);
        y = atan2(-this->data[0][2], sy);
        z = atan2( this->data[0][1], this->data[0][0]);
    }
    else
    {
        x = atan2(-this->data[2][1], this->data[1][1]);
        y = atan2(-this->data[0][2], sy);
        z = 0;
    }

    return EVec3(x,y,z);
}
